#include "config.h"

TTGOClass *ttgo = nullptr;
TFT_eSPI *tft ;

void drawBmp(const char *filename, int16_t x, int16_t y);

// Put bmp1, bmp2 in the directory into the sd card

void setup()
{
    bool ret;
    Serial.begin(115200);
    ttgo = TTGOClass::getWatch();
    ttgo->begin();
    tft = ttgo->tft;
    tft->fillScreen(TFT_BLACK);
    ttgo->openBL();

    tft->setTextDatum(MC_DATUM);
    do {
        ret = ttgo->sdcard_begin();
        if (!ret) {
            tft->setTextColor(TFT_RED);
            tft->drawString("SDCard Init Fail", 120, 120);
            delay(1000);
        }
    } while (!ret);
    tft->fillScreen(TFT_BLACK);
    tft->setTextColor(TFT_GREEN);
    tft->drawString("SDCard Init Done", 120, 120);
    delay(1000);

}

void loop()
{
    drawBmp("/bmp1.bmp", 0, 0);
    delay(1000);
    drawBmp("/bmp2.bmp", 0, 0);
    delay(1000);
}

// These read 16- and 32-bit types from the SD card file.
// BMP data is stored little-endian, Arduino is little-endian too.
// May need to reverse subscript order if porting elsewhere.
uint16_t read16(fs::File &f)
{
    uint16_t result;
    ((uint8_t *)&result)[0] = f.read(); // LSB
    ((uint8_t *)&result)[1] = f.read(); // MSB
    return result;
}

uint32_t read32(fs::File &f)
{
    uint32_t result;
    ((uint8_t *)&result)[0] = f.read(); // LSB
    ((uint8_t *)&result)[1] = f.read();
    ((uint8_t *)&result)[2] = f.read();
    ((uint8_t *)&result)[3] = f.read(); // MSB
    return result;
}


// Bodmers BMP image rendering function
void drawBmp(const char *filename, int16_t x, int16_t y)
{
    if ((x >= tft->width()) || (y >= tft->height())) return;
    fs::File bmpFS;
    // Open requested file on SD card
    bmpFS = SD.open(filename, "r");
    if (!bmpFS) {
        Serial.print("File not found");
        return;
    }
    uint32_t seekOffset;
    uint16_t w, h, row, col;
    uint8_t  r, g, b;
    uint32_t startTime = millis();
    if (read16(bmpFS) == 0x4D42) {
        read32(bmpFS);
        read32(bmpFS);
        seekOffset = read32(bmpFS);
        read32(bmpFS);
        w = read32(bmpFS);
        h = read32(bmpFS);

        if ((read16(bmpFS) == 1) && (read16(bmpFS) == 24) && (read32(bmpFS) == 0)) {
            y += h - 1;

            bool oldSwapBytes = tft->getSwapBytes();
            tft->setSwapBytes(true);
            bmpFS.seek(seekOffset);

            uint16_t padding = (4 - ((w * 3) & 3)) & 3;
            uint8_t lineBuffer[w * 3 + padding];

            for (row = 0; row < h; row++) {

                bmpFS.read(lineBuffer, sizeof(lineBuffer));
                uint8_t  *bptr = lineBuffer;
                uint16_t *tptr = (uint16_t *)lineBuffer;
                // Convert 24 to 16 bit colours
                for (uint16_t col = 0; col < w; col++) {
                    b = *bptr++;
                    g = *bptr++;
                    r = *bptr++;
                    *tptr++ = ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
                }

                // Push the pixel row to screen, pushImage will crop the line if needed
                // y is decremented as the BMP image is drawn bottom up
                tft->pushImage(x, y--, w, 1, (uint16_t *)lineBuffer);
            }
            tft->setSwapBytes(oldSwapBytes);
            Serial.print("Loaded in "); Serial.print(millis() - startTime);
            Serial.println(" ms");
        } else Serial.println("BMP format not recognized.");
    }
    bmpFS.close();
}


